% 系统建模分析
clear;

L0s=0.11:0.01:0.35; % L0变化范围
Ks=zeros(2,6,length(L0s)); % 存放不同L0对应的K

for step=1:length(L0s)
    fprintf('step=%d\n', step);
    % 所需符号量
    syms theta theta1 theta2; % theta1=dTheta, theta2=ddTheta
    syms x x1 x2;
    syms phi phi1 phi2;
    syms T Tp N P Nm Pm Nf t;
    
    % 机器人结构参数(国际单位制)
    R=0.106; 
    L=L0s(step)/2; 
    Lm=L0s(step)/2; 
    l=0; 
    mw=2 * 2.8648; 
    mp=2 * ((0.1+0.1)*2); 
    M= 12.65813;
    Iw=2 * 9721822.23e-9; 
    Ip=2 * (0.4*L0s(step)+0.07); 
    Im=249032349.82e-9;
    g=9.8;
    
    % 进行物理计算
    Nm=M*(x2+(L+Lm)*(theta2*cos(theta)-theta1^2*sin(theta))-l*(phi2*cos(phi)-phi1^2*sin(phi)));
    Pm=M*g+M*((L+Lm)*(-theta1^2*cos(theta)-theta2*sin(theta))-l*(phi1^2*cos(phi)+phi2*sin(phi)));
    N=Nm+mp*(x2+L*(theta2*cos(theta)-theta1^2*sin(theta)));
    P=Pm+mp*g+mp*L*(-theta1^2*cos(theta)-theta2*sin(theta));
    
    equ1=x2-(T-N*R)/(Iw/R+mw*R);
    equ2=(P*L+Pm*Lm)*sin(theta)-(N*L+Nm*Lm)*cos(theta)-T+Tp-Ip*theta2;
    equ3=Tp+Nm*l*cos(phi)+Pm*l*sin(phi)-Im*phi2;
    [x2,theta2,phi2]=solve(equ1,equ2,equ3,x2,theta2,phi2);
    
    % 求得雅克比矩阵，然后得到状态空间方程
    Ja=jacobian([theta1;theta2;x1;x2;phi1;phi2],[theta theta1 x x1 phi phi1]);
    Jb=jacobian([theta1;theta2;x1;x2;phi1;phi2],[T Tp]);
    A=vpa(subs(Ja,[theta theta1 x x1  phi phi1],[0 0 0 0 0 0]));
    B=vpa(subs(Jb,[theta theta1 x x1  phi phi1],[0 0 0 0 0 0]));
    
    % 离散化
    [G,H]=c2d(eval(A),eval(B),0.002);%最后一个参数是离散化的步长(s)

    % 定义权重矩阵Q, R
    Q_theta     = 1;
    Q_theta_dot = 1;
    Q_x         = 500;
    Q_x_dot     = 100;
    Q_phi       = 5000;
    Q_phi_dot   = 1;
    Q=diag([Q_theta Q_theta_dot Q_x Q_x_dot Q_phi Q_phi_dot]);
    R=diag([1 1]);

    % 求解反馈矩阵K
    Ks(:,:,step)=lqr(eval(A),eval(B),Q,R);
end

% 对K的每个元素关于L0进行拟合
K=sym('K',[2 6]);
syms L0;
for x=1:2
    for y=1:6
        p=polyfit(L0s,reshape(Ks(x,y,:),1,length(L0s)),3);
        K(x,y)=p(1)*L0^3+p(2)*L0^2+p(3)*L0+p(4);
    end
end

% 输出到m函数
matlabFunction(K,'File','L2K');


